#include <stdio.h>
#include <string.h>
#include "bsp_weeecorebot.h"
#include "esp_log.h"
#include "bsp_i2c.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

static const char *TAG = "bsp_weeecorebot";

// 添加Weeecore Bot设备到I2C总线
esp_err_t weeecorebot_add_device(i2c_master_bus_handle_t bus_handle, i2c_master_dev_handle_t *dev_handle)
{
    if (!bus_handle || !dev_handle) {
        return ESP_ERR_INVALID_ARG;
    }

    ESP_LOGI(TAG, "Adding Weeecore Bot device to I2C bus...");
    
    // 添加Weeecore Bot设备
    esp_err_t ret = bsp_i2c_add_device(bus_handle, dev_handle, WEEECOREBOT_I2C_ADDRESS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to add Weeecore Bot device: %s", esp_err_to_name(ret));
        return ret;
    }

    ESP_LOGI(TAG, "Weeecore Bot device added successfully");
    return ESP_OK;
}

// 读取设备信息
esp_err_t weeecorebot_read_device_info(i2c_master_dev_handle_t dev_handle, weeecorebot_device_info_t *info)
{
    if (!dev_handle || !info) {
        return ESP_ERR_INVALID_ARG;
    }

    // 发送设备信息查询命令
    uint8_t cmd = CMD_DEVICE_INFO;
    esp_err_t ret = i2c_master_transmit(dev_handle, &cmd, 1, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send device info command: %s", esp_err_to_name(ret));
        return ret;
    }

    // 等待从机处理数据（10ms延时，确保从机端有足够时间处理命令）
    vTaskDelay(pdMS_TO_TICKS(10));

    // 接收响应数据
    uint8_t response[WEEECOREBOT_MAX_DATA_LEN];
    ret = i2c_master_receive(dev_handle, response, WEEECOREBOT_MAX_DATA_LEN, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to receive device info response: %s", esp_err_to_name(ret));
        return ret;
    }

    ESP_LOGI(TAG, "Received device info response");

    // 智能检测数据长度
    size_t data_len = WEEECOREBOT_MAX_DATA_LEN;
    
    // 方法1: 查找字符串结束符
    for (size_t i = 0; i < WEEECOREBOT_MAX_DATA_LEN; i++) {
        if (response[i] == '\0') {
            data_len = i;
            break;
        }
    }
    
    // 方法2: 如果没有找到结束符，查找状态标志
    if (data_len == WEEECOREBOT_MAX_DATA_LEN) {
        // 从后往前查找状态标志
        for (int i = WEEECOREBOT_MAX_DATA_LEN - 1; i >= 0; i--) {
            if (response[i] == RESPONSE_SUCCESS || response[i] == RESPONSE_FAILED) {
                if (response[i] != RESPONSE_SUCCESS) {
                    ESP_LOGE(TAG, "Device info command failed with status: 0x%02X", response[i]);
                    return ESP_FAIL;
                }
                data_len = i; // 状态标志之前的数据长度
                break;
            }
        }
    }
    
    // 方法3: 如果仍然没有找到，使用所有数据
    if (data_len == WEEECOREBOT_MAX_DATA_LEN) {
        data_len = WEEECOREBOT_MAX_DATA_LEN;
    }
    

    // 解析设备信息字符串
    char info_str[64];  // 匹配从机端I2C_TX_BUFFER_SIZE
    if (data_len >= sizeof(info_str)) {
        data_len = sizeof(info_str) - 1;
    }
    memcpy(info_str, response, data_len);
    info_str[data_len] = '\0';


    // 解析格式: "Weeecore_bot_V1.0.0,Build:Oct 23 2025,11:58:06"
    // 使用strtok_r来避免重复调用strtok的问题
    char *saveptr;
    char *token = strtok_r(info_str, "_", &saveptr);
    if (token) {
        strncpy(info->device_name, token, sizeof(info->device_name) - 1);
        info->device_name[sizeof(info->device_name) - 1] = '\0';
    }

    token = strtok_r(NULL, ",", &saveptr);
    if (token) {
        strncpy(info->firmware_version, token, sizeof(info->firmware_version) - 1);
        info->firmware_version[sizeof(info->firmware_version) - 1] = '\0';
    }

    token = strtok_r(NULL, ",", &saveptr);
    if (token) {
        char *date_start = token;
        if (strncmp(token, "Build:", 6) == 0) {
            date_start = token + 6;
        }
        strncpy(info->build_date, date_start, sizeof(info->build_date) - 1);
        info->build_date[sizeof(info->build_date) - 1] = '\0';
    }

    token = strtok_r(NULL, ",", &saveptr);
    if (token) {
        strncpy(info->build_time, token, sizeof(info->build_time) - 1);
        info->build_time[sizeof(info->build_time) - 1] = '\0';
    }

    ESP_LOGI(TAG, "Device: %s, Version: %s, Date: %s, Time: %s", 
             info->device_name, info->firmware_version, info->build_date, info->build_time);
    return ESP_OK;
}

// LED控制
esp_err_t weeecorebot_led_control(i2c_master_dev_handle_t dev_handle, uint8_t led_cmd)
{
    if (!dev_handle) {
        return ESP_ERR_INVALID_ARG;
    }

    // 准备LED控制命令数据
    uint8_t cmd_data[2];
    cmd_data[0] = CMD_LED_CONTROL;
    cmd_data[1] = led_cmd;

    // 调试：打印发送的命令
    ESP_LOGI(TAG, "Sending LED command: 0x%02X 0x%02X", cmd_data[0], cmd_data[1]);

    // 发送LED控制命令
    esp_err_t ret = i2c_master_transmit(dev_handle, cmd_data, 2, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send LED control command: %s", esp_err_to_name(ret));
        return ret;
    }

    // 等待从机处理数据（10ms延时，确保从机端有足够时间处理命令）
    vTaskDelay(pdMS_TO_TICKS(10));

    // 接收响应（只需要1个字节的状态）
    uint8_t response;
    ret = i2c_master_receive(dev_handle, &response, 1, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to receive LED control response: %s", esp_err_to_name(ret));
        return ret;
    }

    // 调试：打印响应状态
    ESP_LOGI(TAG, "LED control response: 0x%02X", response);

    // 检查响应状态
    if (response == RESPONSE_SUCCESS) {
        const char* cmd_names[] = {
            "Unknown", "Turn ON", "Turn OFF", "Disable Blink", "Enable Blink"
        };
        const char* cmd_name = (led_cmd >= 1 && led_cmd <= 4) ? cmd_names[led_cmd] : cmd_names[0];
        ESP_LOGI(TAG, "LED control success: %s", cmd_name);
        return ESP_OK;
    } else {
        ESP_LOGE(TAG, "LED control failed with status: 0x%02X", response);
        return ESP_FAIL;
    }
}

// 读取电池电压
esp_err_t weeecorebot_read_battery_voltage(i2c_master_dev_handle_t dev_handle, float *voltage)
{
    if (!dev_handle || !voltage) {
        return ESP_ERR_INVALID_ARG;
    }

    // 发送电池电压读取命令
    uint8_t cmd = CMD_BATTERY_VOLTAGE;
    esp_err_t ret = i2c_master_transmit(dev_handle, &cmd, 1, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send battery voltage command: %s", esp_err_to_name(ret));
        return ret;
    }

    // 等待从机处理数据（10ms延时，确保从机端有足够时间处理命令）
    vTaskDelay(pdMS_TO_TICKS(10));

    // 接收响应数据（float + 状态标志 = 5字节）
    uint8_t response[8];  // 足够容纳float + 状态标志
    ret = i2c_master_receive(dev_handle, response, 8, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to receive battery voltage response: %s", esp_err_to_name(ret));
        return ret;
    }

    // 检查响应状态（最后一个字节）
    if (response[4] != RESPONSE_SUCCESS) {
        ESP_LOGE(TAG, "Battery voltage command failed with status: 0x%02X", response[4]);
        return ESP_FAIL;
    }

    // 解析float电压值（前4个字节）
    memcpy(voltage, response, sizeof(float));
    
    ESP_LOGI(TAG, "Battery voltage: %.2fV", *voltage);
    return ESP_OK;
}

// 设置巡线传感器阈值
esp_err_t weeecorebot_set_line_patrol_threshold(i2c_master_dev_handle_t dev_handle, uint16_t threshold)
{
    if (!dev_handle) {
        return ESP_ERR_INVALID_ARG;
    }

    // 准备设置阈值命令数据
    uint8_t cmd_data[4];
    cmd_data[0] = CMD_LINE_PATROL;
    cmd_data[1] = LINE_PATROL_CMD_SET_THRESHOLD;
    cmd_data[2] = (threshold >> 8) & 0xFF;  // 高字节
    cmd_data[3] = threshold & 0xFF;         // 低字节

    // 调试：打印发送的命令
    ESP_LOGI(TAG, "Sending line patrol threshold command: 0x%02X 0x%02X 0x%02X 0x%02X", 
             cmd_data[0], cmd_data[1], cmd_data[2], cmd_data[3]);

    // 发送设置阈值命令
    esp_err_t ret = i2c_master_transmit(dev_handle, cmd_data, 4, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send line patrol threshold command: %s", esp_err_to_name(ret));
        return ret;
    }

    // 等待从机处理数据（10ms延时，确保从机端有足够时间处理命令）
    vTaskDelay(pdMS_TO_TICKS(10));

    // 接收响应（只需要1个字节的状态）
    uint8_t response;
    ret = i2c_master_receive(dev_handle, &response, 1, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to receive line patrol threshold response: %s", esp_err_to_name(ret));
        return ret;
    }

    // 调试：打印响应状态
    ESP_LOGI(TAG, "Line patrol threshold response: 0x%02X", response);

    // 检查响应状态
    if (response == RESPONSE_SUCCESS) {
        ESP_LOGI(TAG, "Line patrol threshold set successfully: %d", threshold);
        return ESP_OK;
    } else {
        ESP_LOGE(TAG, "Line patrol threshold set failed with status: 0x%02X", response);
        return ESP_FAIL;
    }
}

// 读取巡线传感器数据
esp_err_t weeecorebot_read_line_patrol_data(i2c_master_dev_handle_t dev_handle, weeecorebot_line_patrol_data_t *data)
{
    if (!dev_handle || !data) {
        return ESP_ERR_INVALID_ARG;
    }

    // 准备读取数据命令
    uint8_t cmd_data[2];
    cmd_data[0] = CMD_LINE_PATROL;
    cmd_data[1] = LINE_PATROL_CMD_READ_DATA;

    // 调试：打印发送的命令
    ESP_LOGI(TAG, "Sending line patrol read command: 0x%02X 0x%02X", cmd_data[0], cmd_data[1]);

    // 发送读取数据命令
    esp_err_t ret = i2c_master_transmit(dev_handle, cmd_data, 2, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send line patrol read command: %s", esp_err_to_name(ret));
        return ret;
    }

    // 等待从机处理数据（10ms延时，确保从机端有足够时间处理命令）
    vTaskDelay(pdMS_TO_TICKS(10));

    // 接收响应数据（4个uint16_t + 1个状态标志 = 9字节）
    uint8_t response[12];  // 足够容纳数据 + 状态标志
    ret = i2c_master_receive(dev_handle, response, 12, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to receive line patrol data response: %s", esp_err_to_name(ret));
        return ret;
    }

    // 检查响应状态（最后一个字节）
    if (response[8] != RESPONSE_SUCCESS) {
        ESP_LOGE(TAG, "Line patrol data read failed with status: 0x%02X", response[8]);
        return ESP_FAIL;
    }

    // 调试：打印原始响应数据
    ESP_LOGI(TAG, "Raw line patrol response: ");
    for (int i = 0; i < 9; i++) {
        ESP_LOGI(TAG, "  [%d]: 0x%02X", i, response[i]);
    }

    // 解析巡线传感器数据（前8个字节，4个uint16_t）
    // 尝试小端序解析（低字节在前）
    for (int i = 0; i < 4; i++) {
        data->sensor_data[i] = response[i*2] | (response[i*2 + 1] << 8);
    }
    
    ESP_LOGI(TAG, "Line patrol data: [%d, %d, %d, %d]", 
             data->sensor_data[0], data->sensor_data[1], 
             data->sensor_data[2], data->sensor_data[3]);
    return ESP_OK;
}

// 电机直接PWM控制
esp_err_t weeecorebot_motor_direct_pwm(i2c_master_dev_handle_t dev_handle, int8_t left_pwm, int8_t right_pwm)
{
    if (!dev_handle) {
        return ESP_ERR_INVALID_ARG;
    }

    // 限制PWM范围在-100~100
    if (left_pwm > 100) left_pwm = 100;
    if (left_pwm < -100) left_pwm = -100;
    if (right_pwm > 100) right_pwm = 100;
    if (right_pwm < -100) right_pwm = -100;

    // 准备直接PWM控制命令数据
    uint8_t cmd_data[4];
    cmd_data[0] = CMD_MOTOR_CONTROL;
    cmd_data[1] = MOTOR_CMD_DIRECT_PWM;
    cmd_data[2] = (uint8_t)left_pwm;
    cmd_data[3] = (uint8_t)right_pwm;

    // 调试：打印发送的命令
    ESP_LOGI(TAG, "Sending motor direct PWM command: 0x%02X 0x%02X 0x%02X 0x%02X", 
             cmd_data[0], cmd_data[1], cmd_data[2], cmd_data[3]);

    // 发送直接PWM控制命令
    esp_err_t ret = i2c_master_transmit(dev_handle, cmd_data, 4, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send motor direct PWM command: %s", esp_err_to_name(ret));
        return ret;
    }

    // 等待从机处理数据（10ms延时，确保从机端有足够时间处理命令）
    vTaskDelay(pdMS_TO_TICKS(10));

    // 接收响应（只需要1个字节的状态）
    uint8_t response;
    ret = i2c_master_receive(dev_handle, &response, 1, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to receive motor direct PWM response: %s", esp_err_to_name(ret));
        return ret;
    }

    // 调试：打印响应状态
    ESP_LOGI(TAG, "Motor direct PWM response: 0x%02X", response);

    // 检查响应状态
    if (response == RESPONSE_SUCCESS) {
        ESP_LOGI(TAG, "Motor direct PWM control success: Left=%d, Right=%d", left_pwm, right_pwm);
        return ESP_OK;
    } else {
        ESP_LOGE(TAG, "Motor direct PWM control failed with status: 0x%02X", response);
        return ESP_FAIL;
    }
}

// 电机平滑PWM控制
esp_err_t weeecorebot_motor_smooth_pwm(i2c_master_dev_handle_t dev_handle, int8_t left_pwm, int8_t right_pwm, uint8_t ramp_time)
{
    if (!dev_handle) {
        return ESP_ERR_INVALID_ARG;
    }

    // 限制PWM范围在-100~100
    if (left_pwm > 100) left_pwm = 100;
    if (left_pwm < -100) left_pwm = -100;
    if (right_pwm > 100) right_pwm = 100;
    if (right_pwm < -100) right_pwm = -100;

    // 准备平滑PWM控制命令数据
    uint8_t cmd_data[5];
    cmd_data[0] = CMD_MOTOR_CONTROL;
    cmd_data[1] = MOTOR_CMD_SMOOTH_PWM;
    cmd_data[2] = (uint8_t)left_pwm;
    cmd_data[3] = (uint8_t)right_pwm;
    cmd_data[4] = ramp_time;

    // 调试：打印发送的命令
    ESP_LOGI(TAG, "Sending motor smooth PWM command: 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X", 
             cmd_data[0], cmd_data[1], cmd_data[2], cmd_data[3], cmd_data[4]);

    // 发送平滑PWM控制命令
    esp_err_t ret = i2c_master_transmit(dev_handle, cmd_data, 5, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send motor smooth PWM command: %s", esp_err_to_name(ret));
        return ret;
    }

    // 等待从机处理数据（10ms延时，确保从机端有足够时间处理命令）
    vTaskDelay(pdMS_TO_TICKS(10));

    // 接收响应（只需要1个字节的状态）
    uint8_t response;
    ret = i2c_master_receive(dev_handle, &response, 1, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to receive motor smooth PWM response: %s", esp_err_to_name(ret));
        return ret;
    }

    // 调试：打印响应状态
    ESP_LOGI(TAG, "Motor smooth PWM response: 0x%02X", response);

    // 检查响应状态
    if (response == RESPONSE_SUCCESS) {
        ESP_LOGI(TAG, "Motor smooth PWM control success: Left=%d, Right=%d, Ramp=%dms", 
                 left_pwm, right_pwm, ramp_time * 10);
        return ESP_OK;
    } else {
        ESP_LOGE(TAG, "Motor smooth PWM control failed with status: 0x%02X", response);
        return ESP_FAIL;
    }
}

// 电机速度PID控制
esp_err_t weeecorebot_motor_speed_pid(i2c_master_dev_handle_t dev_handle, int8_t left_rpm, int8_t right_rpm)
{
    if (!dev_handle) {
        return ESP_ERR_INVALID_ARG;
    }

    // 限制RPM范围在-60~60
    if (left_rpm > 60) left_rpm = 60;
    if (left_rpm < -60) left_rpm = -60;
    if (right_rpm > 60) right_rpm = 60;
    if (right_rpm < -60) right_rpm = -60;

    // 准备速度PID控制命令数据
    uint8_t cmd_data[4];
    cmd_data[0] = CMD_MOTOR_CONTROL;
    cmd_data[1] = MOTOR_CMD_SPEED_PID;
    cmd_data[2] = (uint8_t)left_rpm;
    cmd_data[3] = (uint8_t)right_rpm;

    // 调试：打印发送的命令
    ESP_LOGI(TAG, "Sending motor speed PID command: 0x%02X 0x%02X 0x%02X 0x%02X", 
             cmd_data[0], cmd_data[1], cmd_data[2], cmd_data[3]);

    // 发送速度PID控制命令
    esp_err_t ret = i2c_master_transmit(dev_handle, cmd_data, 4, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send motor speed PID command: %s", esp_err_to_name(ret));
        return ret;
    }

    // 等待从机处理数据（10ms延时，确保从机端有足够时间处理命令）
    vTaskDelay(pdMS_TO_TICKS(10));

    // 接收响应（只需要1个字节的状态）
    uint8_t response;
    ret = i2c_master_receive(dev_handle, &response, 1, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to receive motor speed PID response: %s", esp_err_to_name(ret));
        return ret;
    }

    // 调试：打印响应状态
    ESP_LOGI(TAG, "Motor speed PID response: 0x%02X", response);

    // 检查响应状态
    if (response == RESPONSE_SUCCESS) {
        ESP_LOGI(TAG, "Motor speed PID control success: Left=%d RPM, Right=%d RPM", left_rpm, right_rpm);
        return ESP_OK;
    } else {
        ESP_LOGE(TAG, "Motor speed PID control failed with status: 0x%02X", response);
        return ESP_FAIL;
    }
}

// 电机位置PID控制
esp_err_t weeecorebot_motor_position_pid(i2c_master_dev_handle_t dev_handle, int32_t left_position, int32_t right_position)
{
    if (!dev_handle) {
        return ESP_ERR_INVALID_ARG;
    }

    // 准备位置PID控制命令数据（10字节：命令+子命令+8字节位置数据）
    uint8_t cmd_data[10];
    cmd_data[0] = CMD_MOTOR_CONTROL;
    cmd_data[1] = MOTOR_CMD_POSITION_PID;
    
    // 将32位位置值分解为4个字节（大端序）
    cmd_data[2] = (left_position >> 24) & 0xFF;   // 左电机位置高字节
    cmd_data[3] = (left_position >> 16) & 0xFF;
    cmd_data[4] = (left_position >> 8) & 0xFF;
    cmd_data[5] = left_position & 0xFF;           // 左电机位置低字节
    
    cmd_data[6] = (right_position >> 24) & 0xFF;  // 右电机位置高字节
    cmd_data[7] = (right_position >> 16) & 0xFF;
    cmd_data[8] = (right_position >> 8) & 0xFF;
    cmd_data[9] = right_position & 0xFF;          // 右电机位置低字节

    // 调试：打印发送的命令
    ESP_LOGI(TAG, "Sending motor position PID command: 0x%02X 0x%02X", cmd_data[0], cmd_data[1]);
    ESP_LOGI(TAG, "Left position: %ld (0x%08lX)", left_position, left_position);
    ESP_LOGI(TAG, "Right position: %ld (0x%08lX)", right_position, right_position);

    // 发送位置PID控制命令
    esp_err_t ret = i2c_master_transmit(dev_handle, cmd_data, 10, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send motor position PID command: %s", esp_err_to_name(ret));
        return ret;
    }

    // 等待从机处理数据（10ms延时，确保从机端有足够时间处理命令）
    vTaskDelay(pdMS_TO_TICKS(10));

    // 接收响应（只需要1个字节的状态）
    uint8_t response;
    ret = i2c_master_receive(dev_handle, &response, 1, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to receive motor position PID response: %s", esp_err_to_name(ret));
        return ret;
    }

    // 调试：打印响应状态
    ESP_LOGI(TAG, "Motor position PID response: 0x%02X", response);

    // 检查响应状态
    if (response == RESPONSE_SUCCESS) {
        ESP_LOGI(TAG, "Motor position PID control success: Left=%ld, Right=%ld", left_position, right_position);
        return ESP_OK;
    } else {
        ESP_LOGE(TAG, "Motor position PID control failed with status: 0x%02X", response);
        return ESP_FAIL;
    }
}

// 读取电机状态信息
esp_err_t weeecorebot_motor_read_status(i2c_master_dev_handle_t dev_handle, weeecorebot_motor_status_t *status)
{
    if (!dev_handle || !status) {
        return ESP_ERR_INVALID_ARG;
    }

    // 准备读取电机状态命令数据
    uint8_t cmd_data[2];
    cmd_data[0] = CMD_MOTOR_CONTROL;
    cmd_data[1] = MOTOR_CMD_READ_STATUS;

    // 调试：打印发送的命令
    //ESP_LOGI(TAG, "Sending motor read status command: 0x%02X 0x%02X", cmd_data[0], cmd_data[1]);

    // 发送读取电机状态命令
    esp_err_t ret = i2c_master_transmit(dev_handle, cmd_data, 2, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        //ESP_LOGE(TAG, "Failed to send motor read status command: %s", esp_err_to_name(ret));
        return ret;
    }

    // 等待从机处理数据（20ms延时，电机状态读取需要更长时间）
    vTaskDelay(pdMS_TO_TICKS(12));

    // 接收响应数据（26字节数据 + 1字节状态标志 = 27字节）
    uint8_t response[32];  // 足够容纳数据 + 状态标志
    ret = i2c_master_receive(dev_handle, response, 32, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        //ESP_LOGE(TAG, "Failed to receive motor status response: %s", esp_err_to_name(ret));
        return ret;
    }

    // // 调试：打印原始响应数据
    // ESP_LOGI(TAG, "Raw motor status response (first 8 bytes): ");
    // for (int i = 0; i < 8; i++) {
    //     ESP_LOGI(TAG, "  [%d]: 0x%02X", i, response[i]);
    // }

    // 检查响应状态（最后一个字节）
    if (response[26] != RESPONSE_SUCCESS) {
        // ESP_LOGE(TAG, "Motor status read failed with status: 0x%02X", response[26]);
        // ESP_LOGE(TAG, "Response length: 27 bytes, last byte: 0x%02X", response[26]);
        return ESP_FAIL;
    }

    // 解析电机状态数据
    // 位置数据（8字节：左4字节 + 右4字节）
    memcpy(&status->left_position, &response[0], 4);
    memcpy(&status->right_position, &response[4], 4);
    
    // 滤波速度数据（8字节：左4字节 + 右4字节）
    memcpy(&status->left_speed, &response[8], 4);
    memcpy(&status->right_speed, &response[12], 4);
    
    // 原始速度数据（8字节：左4字节 + 右4字节）
    memcpy(&status->left_raw_speed, &response[16], 4);
    memcpy(&status->right_raw_speed, &response[20], 4);
    
    // 方向数据（2字节：左1字节 + 右1字节）
    status->left_direction = response[24];
    status->right_direction = response[25];
    
    // ESP_LOGI(TAG, "Motor status - L_pos:%ld, R_pos:%ld, L_spd:%.1f, R_spd:%.1f, L_raw:%.1f, R_raw:%.1f, L_dir:%d, R_dir:%d",
    //          status->left_position, status->right_position, 
    //          status->left_speed, status->right_speed,
    //          status->left_raw_speed, status->right_raw_speed,
    //          status->left_direction, status->right_direction);
    
    return ESP_OK;
}

// 设置电机角度目标
esp_err_t weeecorebot_motor_set_angle_target(i2c_master_dev_handle_t dev_handle, float angle_target)
{
    if (!dev_handle) {
        return ESP_ERR_INVALID_ARG;
    }

    // 准备设置角度目标命令数据
    uint8_t cmd_data[4];
    cmd_data[0] = CMD_MOTOR_CONTROL;
    cmd_data[1] = MOTOR_CMD_SET_ANGLE_TARGET;
    
    // 将float角度值转换为16位整数（假设角度范围-180~180度，精度0.1度）
    // 将角度值乘以10并转换为整数，范围-1800~1800
    int16_t angle_int = (int16_t)(angle_target * 10.0f);
    
    // 限制角度范围
    if (angle_int > 1800) angle_int = 1800;
    if (angle_int < -1800) angle_int = -1800;
    
    // 将16位整数分解为2个字节（大端序）
    cmd_data[2] = (angle_int >> 8) & 0xFF;   // 高字节
    cmd_data[3] = angle_int & 0xFF;          // 低字节

    // 调试：打印发送的命令
    ESP_LOGI(TAG, "Sending motor set angle target command: 0x%02X 0x%02X", cmd_data[0], cmd_data[1]);
    ESP_LOGI(TAG, "Angle target: %.1f degrees (0x%04X)", angle_target, (uint16_t)angle_int);

    // 发送设置角度目标命令
    esp_err_t ret = i2c_master_transmit(dev_handle, cmd_data, 4, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send motor set angle target command: %s", esp_err_to_name(ret));
        return ret;
    }

    // 等待从机处理数据（10ms延时，确保从机端有足够时间处理命令）
    vTaskDelay(pdMS_TO_TICKS(10));

    // 接收响应（只需要1个字节的状态）
    uint8_t response;
    ret = i2c_master_receive(dev_handle, &response, 1, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to receive motor set angle target response: %s", esp_err_to_name(ret));
        return ret;
    }

    // 调试：打印响应状态
    ESP_LOGI(TAG, "Motor set angle target response: 0x%02X", response);

    // 检查响应状态
    if (response == RESPONSE_SUCCESS) {
        ESP_LOGI(TAG, "Motor set angle target success: %.1f degrees", angle_target);
        return ESP_OK;
    } else {
        ESP_LOGE(TAG, "Motor set angle target failed with status: 0x%02X", response);
        return ESP_FAIL;
    }
}

// 控制电机直线行驶
esp_err_t weeecorebot_motor_line_control(i2c_master_dev_handle_t dev_handle, uint8_t direction)
{
    if (!dev_handle) {
        return ESP_ERR_INVALID_ARG;
    }

    // 准备直线控制命令数据
    uint8_t cmd_data[3];
    cmd_data[0] = CMD_MOTOR_CONTROL;
    cmd_data[1] = MOTOR_CMD_LINE_CONTROL;
    cmd_data[2] = direction;

    // 调试：打印发送的命令
    ESP_LOGI(TAG, "Sending motor line control command: 0x%02X 0x%02X 0x%02X", 
             cmd_data[0], cmd_data[1], cmd_data[2]);

    // 发送直线控制命令
    esp_err_t ret = i2c_master_transmit(dev_handle, cmd_data, 3, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send motor line control command: %s", esp_err_to_name(ret));
        return ret;
    }

    // 等待从机处理数据（10ms延时，确保从机端有足够时间处理命令）
    vTaskDelay(pdMS_TO_TICKS(10));

    // 接收响应（只需要1个字节的状态）
    uint8_t response;
    ret = i2c_master_receive(dev_handle, &response, 1, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to receive motor line control response: %s", esp_err_to_name(ret));
        return ret;
    }

    // 调试：打印响应状态
    ESP_LOGI(TAG, "Motor line control response: 0x%02X", response);

    // 检查响应状态
    if (response == RESPONSE_SUCCESS) {
        ESP_LOGI(TAG, "Motor line control success: direction=%d", direction);
        return ESP_OK;
    } else {
        ESP_LOGE(TAG, "Motor line control failed with status: 0x%02X", response);
        return ESP_FAIL;
    }
}

// 停止所有电机
esp_err_t weeecorebot_motor_stop_all(i2c_master_dev_handle_t dev_handle)
{
    if (!dev_handle) {
        return ESP_ERR_INVALID_ARG;
    }

    // 准备停止所有电机命令数据
    uint8_t cmd_data[2];
    cmd_data[0] = CMD_MOTOR_CONTROL;
    cmd_data[1] = MOTOR_CMD_STOP_ALL;

    // 调试：打印发送的命令
    ESP_LOGI(TAG, "Sending motor stop all command: 0x%02X 0x%02X", cmd_data[0], cmd_data[1]);

    // 发送停止所有电机命令
    esp_err_t ret = i2c_master_transmit(dev_handle, cmd_data, 2, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send motor stop all command: %s", esp_err_to_name(ret));
        return ret;
    }

    // 等待从机处理数据（20ms延时，停止电机可能需要更长时间）
    vTaskDelay(pdMS_TO_TICKS(20));

    // 接收响应（只需要1个字节的状态）
    uint8_t response;
    ret = i2c_master_receive(dev_handle, &response, 1, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to receive motor stop all response: %s", esp_err_to_name(ret));
        return ret;
    }

    // 调试：打印响应状态
    ESP_LOGI(TAG, "Motor stop all response: 0x%02X", response);

    // 检查响应状态
    if (response == RESPONSE_SUCCESS) {
        ESP_LOGI(TAG, "Motor stop all success");
        return ESP_OK;
    } else {
        ESP_LOGE(TAG, "Motor stop all failed with status: 0x%02X", response);
        return ESP_FAIL;
    }
}

// 重置电机位置
esp_err_t weeecorebot_motor_reset_position(i2c_master_dev_handle_t dev_handle)
{
    if (!dev_handle) {
        return ESP_ERR_INVALID_ARG;
    }

    // 准备重置位置命令数据
    uint8_t cmd_data[2];
    cmd_data[0] = CMD_MOTOR_CONTROL;
    cmd_data[1] = MOTOR_CMD_RESET_POSITION;

    // 调试：打印发送的命令
    ESP_LOGI(TAG, "Sending motor reset position command: 0x%02X 0x%02X", cmd_data[0], cmd_data[1]);

    // 发送重置位置命令
    esp_err_t ret = i2c_master_transmit(dev_handle, cmd_data, 2, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to send motor reset position command: %s", esp_err_to_name(ret));
        return ret;
    }

    // 等待从机处理数据（10ms延时，确保从机端有足够时间处理命令）
    vTaskDelay(pdMS_TO_TICKS(10));

    // 接收响应（只需要1个字节的状态）
    uint8_t response;
    ret = i2c_master_receive(dev_handle, &response, 1, WEEECOREBOT_I2C_TIMEOUT_MS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to receive motor reset position response: %s", esp_err_to_name(ret));
        return ret;
    }

    // 调试：打印响应状态
    ESP_LOGI(TAG, "Motor reset position response: 0x%02X", response);

    // 检查响应状态
    if (response == RESPONSE_SUCCESS) {
        ESP_LOGI(TAG, "Motor reset position success");
        return ESP_OK;
    } else {
        ESP_LOGE(TAG, "Motor reset position failed with status: 0x%02X", response);
        return ESP_FAIL;
    }
}
